#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import time
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode
from geometry_msgs.msg import PoseStamped, TwistStamped

current_state = None

# 回调函数，用于更新无人机的当前状态
def state_cb(state_data):
    global current_state
    current_state = state_data

# 初始化ROS节点
rospy.init_node("move_takeoff", anonymous=True)

# 订阅无人机的状态信息
state_sub = rospy.Subscriber("/mavros/state", State, state_cb)

# 发布目标位置的主题
pose_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10)

# 发布速度命令的主题
vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10)

# 服务客户端，解锁无人机
arming_client = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)

# 服务客户端，设置飞行模式
set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)

# 等待建立连接
while not rospy.is_shutdown() and current_state is None:
    rospy.sleep(1)

# 设置目标姿态消息
target_pose = PoseStamped()
target_pose.pose.position.x = 0
target_pose.pose.position.y = 0
target_pose.pose.position.z = 3  # 起飞高度 3米

# 设置目标速度消息
target_vel = TwistStamped()

# 在 OFFBOARD 模式下需要先发送一些目标点
for _ in range(100):
    pose_pub.publish(target_pose)
    rospy.sleep(0.05)

# 切换到 OFFBOARD 模式
set_mode_client(base_mode=0, custom_mode="OFFBOARD")

# 解锁无人机
arming_client(True)

# 起飞到3米高度
target_pose.pose.position.x = 0
target_pose.pose.position.y = 0
target_pose.pose.position.z = 3  # 起飞高度 3米
for _ in range(100):
    pose_pub.publish(target_pose)
    rospy.sleep(0.05)
rospy.loginfo("takeoff complete")

# # 向前飞行 10米
# target_pose.pose.position.x = 20
# for _ in range(200):
#     pose_pub.publish(target_pose)
#     rospy.sleep(0.05)

# # 降落
# target_pose.pose.position.z = 0
# for _ in range(100):
#     pose_pub.publish(target_pose)
#     rospy.sleep(0.05)

# # 上锁无人机
# arming_client(False)

# rospy.loginfo("Landing complete")

